/* -*- Mode: C; indent-tabs-mode: t; c-basic-offset: 4; tab-width: 4 -*- */
/*
 * Localization_ekf
/bin/bash: line 1: cd: /home/giovanniarchetti/Desktop/ekf_slam: No such file or directory
/bin/bash: line 45: cd: /home/giovanniarchetti/Desktop/ekf_slam: No such file or directory
 * Copyright (C)  2012 <>
 * 
 * Localization_ekf is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * Localization_ekf is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "error-test.h"
#include <iostream>
#include <cstdlib>
#include <time.h>
#include <stdio.h>
#include <math.h>
#include <vector>
#include <libplayerc++/playerc++.h>
#include "args.h"
#define RAYS 32
#include <time.h>
#include <signal.h>
#include <ctime>	
#include <sys/time.h>
#include "ekfslam.cpp"

using namespace std;
using namespace PlayerCc;
FILE *posoutput;
FILE *posoutput1;
FILE *laseroutput;
FILE *lm;
FILE *mapout;

//goto definitions
int N_VIA=3;
int N_giri = 0;
double viapointX[]={3,3, -3, -3,3};
double viapointY[]={1,-2,-2,2,2};
int vp=0;
int ig=0;
bool finish = false;

int main(int argc, char** argv){
	
posoutput=fopen("odoDataGPS.txt","w");
posoutput1=fopen("odoDataERR2.txt","w");
	//laseroutput=fopen("Laser.txt","w");
	mapout=fopen("map.txt","w");
	lm=fopen("LM.txt","w");


//These are initialised before the infinite loop
PlayerClient robot(gHostname, gPort);	//This is needed in every player client application
Position2dProxy pp(&robot, gIndex);	//This is needed in every player client application
LaserProxy lp(&robot, gIndex);		//This is needed in every player client application


double *laserdata;			//Laser data array
double x,y,yaw,oldx,oldy,oldyaw;	//Variables used to find change in displacement d_d and change in orientation d_t
robot.Read();				//Obtain DR and laser data
oldx=pp.GetXPos();				//Alternatively, you can use v*dt and w*dt if you know the sample time.
oldy=pp.GetYPos();
oldyaw=pp.GetYaw();


double xest=pp.GetXPos();				//Alternatively, you can use v*dt and w*dt if you know the sample time.
double yest=pp.GetYPos();
double yawest=pp.GetYaw();
	int i=0;
while(!finish){
	i=i+1;

try{

// errore odometrico viene aggiunto ad ogni passo e dovrebbe aumentare (sommarsi) non funziona
 
	
//These are called during the inifinite recursive loop
robot.Read();				//Obtain DR and laser data
x=pp.GetXPos();				//Alternatively, you can use v*dt and w*dt if you know the sample time.
y=pp.GetYPos();
yaw=pp.GetYaw();

	
        double cx=x-oldx;
		double cy=y-oldy;
		double d_d=sqrt(cx*cx+cy*cy);
		double d_t=yaw-oldyaw;
	printf("%4.4f\n ",d_d );
   float n=rand() %100;
  n=n*0.01;
 xest=oldx+ (d_d+n)*cos(d_t+n);
 yest=oldy+ (d_d+n)*sin(d_t+n);
 yawest= oldyaw +d_t;	

	
//laserdata=new double[ekf.laserproxies];	//Set blank array equal to number of proxies in the laser
//for(int j=0;j<ekf.laserproxies;j++){
//	laserdata[j]=lp.GetRange(j);
//}


//logdata
	
fprintf(posoutput,"%f %f %f\n",x,y,yaw);
fprintf(posoutput1,"%f %f %f\n",xest,yest,yawest);

oldx=x;oldy=y;oldyaw=yaw;
//Update map and log data
		//for(int j=3;j<ekf.xest.nRow();j++){
		//	fprintf(lm,"%f ",ekf.xest.val[j][0]);
		//}

// goto commands . . . . .  . . . .. . . . .  . . . . 
if (viapointX[vp] < x+0.2 && viapointX[vp] > x-0.2  && viapointY[vp] < y+0.2 && viapointY[vp] > y-0.2 ){
     vp=vp+1;
std::cout << "goto starting, target: " << viapointX[vp]
              << ", " << viapointY[vp] << std::endl;
std::cout << "indice target i: " << vp << std::endl;
std::cout << "giro " << ig+1 << std::endl;
}
if (vp<N_VIA){

	 robot.Read();
     pp.GoTo(viapointX[vp],viapointY[vp],0);
}

if(vp>=N_VIA && ig<N_giri){
ig=ig+1;
vp=0;
}

if(vp>=N_VIA && ig>=N_giri){
finish= true;
}

}
  catch (PlayerCc::PlayerError & e)
  {
    std::cerr << e << std::endl;
    return -1;
  }

}//THE END loop

fclose(lm);
fclose(posoutput);

}
	


